Method of force estimation for a minimally invasive medical system and corresponding system

ABSTRACT

A method of operating a minimally invasive medical system, including positioning a tip of a surgical instrument through an incision into a body cavity, the surgical instrument having an instrument shaft with a longitudinal instrument axis, mounting the surgical instrument to a manipulator, the manipulator including a force/torque sensor, moving the minimally invasive instrument along a first axis and a second axis, each of the first and second axes perpendicular to the instrument axis, until a reaction force along each axis is below a given threshold, and defining and axis coordinates of the external fulcrum at a location where the reaction forces along both axes are below the given threshold, and determining an axis coordinate position of the external fulcrum along the instrument axis by pivoting the instrument with respect to its tip until a sufficient contact force is reached, and determining the axis coordinate position using the lever principle.

CROSS REFERENCE TO RELATED APPLICATIONS

This application is a continuation of U.S. patent application Ser. No.16/730,903 filed on Dec. 30, 2019, which was a divisional of U.S. patentapplication Ser. No. 15/860,616 filed on Jan. 02, 2018, which granted asU.S. Pat. No. 10,518,419 on Dec. 31, 2019, which was a divisional ofU.S. patent application Ser. No. 12/447,335 filed on Apr. 27, 2009,which granted as U.S. Pat. No. 9,855,662 on Jan. 02, 2018 and which wasfiled under 35 USC 371 as the U.S. national phase of InternationalPatent Application Number PCT/EP2007/061494 filed on Oct. 25, 2007,which claimed priority to European Patent Application Number 06122937.3filed on Oct. 25, 2006, all of which said applications are incorporatedherein by reference in their entirety.

TECHNICAL FIELD

The present invention generally relates to the field of minimallyinvasive medical procedures, including surgery and diagnosticprocedures. More particularly, the invention concerns a method and asystem for force estimation that are capable of determining forcesexerted onto a patient, especially by the tip of a minimally invasiveinstrument, but also at the level of the access port for the instrumentinto the patient body.

INTRODUCTION

It is well known that minimally invasive interventions have the benefitof reducing the amount of extraneous tissue that is damaged duringdiagnostic or surgical procedures. This results in shorter patientrecovery time, less discomfort, less deleterious side effects and lowercosts of the hospital stay. Nowadays, in general surgery, urology,gynecology and cardiology specialties, there is an increase of theamount of interventions carried out by minimally invasive techniques,such as laparoscopic techniques.

Manual minimally invasive techniques in general, and laparoscopy inparticular, put stringent requirements on the surgeon carrying out theoperation. The surgeon operates in an uncomfortable and tiring posture,with a limited field of view, reduced dexterity and poor tactileperception. To these problems adds the fact that surgeons often have tocarry out several consecutive interventions per day, each interventionlasting e.g. from 30 minutes to several hours. In spite of the inherentdifficulties, the trend towards minimally invasive procedures isexpected to increase further in the coming years due to an increasingaverage age of the population and pressure of costs in the medicalfield.

In laparoscopy for example, surgeons are obviously required to be asprecise in his moves as in laparotomy. Manipulating long-shaftinstruments with motion dexterity reduced to four degrees of freedomabout a fulcrum (pivot point) at the instrument access port (also calledtrocar), i.e. at the incision in the patient body, is not alleviatingtheir task. Complications arise inter-alia by the fact that the requiredposture is often tiresome and reduces the already limited perception ofinteracting forces between instrument and tissues. As a result, motorialcapabilities of a surgeon normally decay after 20-30 minutes, such thatamong others trembling, loss of accuracy and loss of tactile sensitivityoccur with the resulting risks for the patient. Therefore, new computerand/or robot assisted technologies, such as Minimally Invasive RoboticSurgery (MIRS), are emerging. These technologies aim at improvingefficiency, quality and safety of intervention.

BACKGROUND ART

In view of the above, MIRS has known significant development during thelast decade. Two representative commercial robotic systems are thesystem known by the trademark ‘DA VINCI’ developed by Intuitive SurgicalInc., Sunnyvale, Calif. and the system known by the trademark ‘ZEUS’originally developed by Computer Motion Inc., Goleta, Calif. The systemknown by the name ‘DA VINCI’ is described among others by Moll et al. inU.S. Pat. Nos. 6,659,939; 6,837,883 and other patent documents of thesame assignee. The system known by the name ‘ZEUS’ is described amongothers by Wang et al. in U.S. Pat. Nos. 6,102,850; 5,855,583; 5,762,458;5,515,478 and other patent documents assigned to Computer Motion Inc.,Goleta, Calif.

These teleoperated robotic systems permit to control surgicalinterventions either directly from the operation theatre or from aremote site, generally using 2-dimensional or 3-dimensional visualfeedback only. In either case, the tiring posture of the surgeon iseliminated. Furthermore, these systems tend to give the surgeon thefeeling to work in open conditions, e.g. as in laparotomy, and eliminatethe aforementioned tiresome posture.

Currently available teleoperated MIS systems typically do not offer truetactile force feedback (referred to as force feedback below) on theconsole by means of which the surgeon commands the robot(s). Hence thesurgeon lacks a true haptic feeling of the forces exerted onto organsand tissues. With such systems, the surgeon has to rely on visualfeedback and on his experience to limit interaction of instruments withthe intra-patient environment. In this respect, research work has beendone concerning a computer-assisted sensorless force feedback systembased on the concept that a computer could reproduce what a surgeonskilled in manual MIS procedures is capable of In other words, acomputer could estimate forces from deformations observed by vision. Anexample of such attempts is found in: “Force feedback using vision”;Kennedy, C. and Desai, J. P.; International Conference on AdvancedRobotics; Coimbra, Portugal, 2003. Such systems have however not yetreach a commercially viable state.

As will be appreciated, accurate force feedback is considered a crucialfeature to ensure operation safety and to improve the quality ofprocedures carried out with machine assisted minimally invasive systems.Therefore, force feedback is believed to be of paramount importance forteleoperated interventions.

At the instrument tip level, force sensing allows for example palpationof organs and tissues, which is highly desirable in diagnosticprocedures and for identifying critical areas e.g. with arteries. Otherpossible enhancements consist in the limitation of stretching tension onsutures and the limitation of exerted forces on tissues according to thetype and specific phase of the intervention. In practice, contact forcescan be kept below a given threshold by increasing motion scales,stopping the manipulator motion, or increasing force feedback on themaster device. Furthermore, force sensing would permit to workintuitively with an instrument that is not in the field of view of theendoscope camera, e.g. when the surgeon assistant holds an organ awayfrom the operation field.

At the access port level, force sensing would be beneficial in order tomonitor and consequently reduce forces applied by the instrument at theincision for the access port. These forces are the main cause ofincision wear that can lead to loss of abdominal pressure, release ofthe trocar, and increased intervention time due to the need to recoverthe situation. These detrimental forces are mainly caused by theinaccurate location of the instrument fulcrum (pivot point), asdetermined by the system and modified due to variations ofintra-abdominal pressure, with respect to the patient incision but alsoby motion drifts of the (robot) manipulator due to its positioninginaccuracy. In manual interventions, these wearing forces are lesspronounced because of the human capability to intuitively adjust handmotion with respect to the optimal pivot point in the incision.

To overcome the trocar-release problem, the aforementioned DA VINCIsystem for example, uses a trocar attached to the manipulator wrist atthe extremity of the instrument insertion/extraction slide. Thissolution does not reduce the risk the incision wear and does not improvethe loss of abdominal pressure.

In order to overcome the latter problem at the trocar level, aforce-feedback adaptive controller, which is capable of automaticallyadjusting the fulcrum point of a robot manipulator on a plane tangent tothe abdomen of the patient, has been developed and described in thepaper “Achieving High Precision Laparoscopic Manipulation ThroughAdaptive Force Control”; Krupa, A. Morel, G. De Mathellin M.;Proceedings of the 2002 IEEE Intern. Conference on Robotics andAutomation; Washington D.C., May 2002. In this approach, a sensor on theend-effector of a robot in combination with a force controller is usedto explicitly regulate the lateral forces exerted onto the trocar, whichtogether with the abdominal wall defines the fulcrum, towards zero. Thismethod and system are not capable of determining the forces at the tipof the instrument inserted through the trocar. Instead, the interactionforce at the instrument tip is assumed to be negligible. Therefore, thismethod can be satisfactorily used only with an endoscope manipulatorthat does not have any other contact point with the patient.

A different approach is described in the paper: “Development of actuatedand sensor integrated forceps for minimally invasive robotic surgery”;B. Kübler, U. Seibold and G. Hirzinger; Jahrestagung der DeutschenGesellschaft für Computer-und Roboterassistierte Chirurgie (CURAC),October 2004. This paper describes a miniaturized 6DOF force/torquesensor to be installed at the tip of a minimally invasive instrument.This sensor enables accurate sensing of the forces exerted by theinstrument tip and corresponding force feedback. This concept hasseveral drawbacks however, among which manufacturing and installationcost, the lack of robustness in autoclave sterilization, and EMIshielding issues when combined with powered instruments. As will beunderstood, a dedicated sensor has to be provided on every instrumentwhen using this approach. A similar approach has been described in thepaper: “A miniature microsurgical instrument tip force sensor forenhanced force feedback during robot-assisted manipulation”; Berkelman,P. J., Whitcomb, L. L., Taylor, R. H., and Jensen, P.; IEEE Transactionson Robotics and Automation, October 2003.

A different approach, which does not require a tip mounted sensor onevery instrument has been described in the paper “A New Robot for ForceControl in Minimally Invasive Surgery”; Zemiti N., Ortmaier T. et MorelG.; IEEE/RSJ International Conference on Intelligent Robots and Systems,Japan, 2004. This paper describes a robot and force sensor arrangementthat can measure the distal organ-instrument interaction with a sensorplaced on the trocar. Even though, in this approach, the sensor is notmounted on the instrument itself and is therefore subject to lowerminiaturization and sterilization constraints, this solution stillrequires modified trocars with sensor equipment capable of resistingsterilization. A further approach designed for MIS, as disclosed inpatent application WO 2005/039835, uses a master/slave architecture withtwo PHANTOM® haptic devices developed by SensAble Technologies, Woburn,Mass. This system comprises a first PHANTOM device integrated into aslave subsystem and serving as manipulator for an instrument incombination with an effector sub-assembly that is configured for holdingand mounting an off-the shelf instrument tip of a minimally invasiveinstrument such as graspers, dissectors, scissors, etc. to the firstPHANTOM device. In operation, the minimally invasive instrument has afirst end mounted to the effector sub-assembly and a second end locatedbeyond an external fulcrum that limits the instrument in motion. Inorder to provide measurement of the force vector (f_(x), f_(y), f_(z))and the moment (τ_(z)) at the end of the instrument tip, a custom madearrangement of various strain gauges is provided. Furthermore, thesystem comprises one or more personal computers with applicationprograms for controlling and serving the first PHANTOM device of theslave subsystem and a second PHANTOM device of the master subsystem.

TECHNICAL PROBLEM

It is an object of the present invention to provide a method and systemthat permit to estimate the force exerted onto, respectively by, theinstrument tip in cost-effective and efficient manner while avoiding theneed for trocar and/or instrument tip mounted sensors.

GENERAL DESCRIPTION OF THE INVENTION

To achieve this object, the invention proposes a method of forceestimation. and a minimally invasive medical system, in particular alaparoscopic system, adapted to perform this method. The systemcomprises a manipulator, e.g. a robot manipulator, that has an effectorunit equipped with a six degrees-of-freedom (6-DOF or 6-axes)force/torque sensor. The effector unit is configured for holding aminimally invasive instrument manned thereto. In normal use, a first endof the instrument is mounted to the effector unit and the opposite,second end of the instrument is located beyond an external fulcrum(pivot point kinematic constraint) that limits the instrument in motion.In general, the fulcrum is located within an access port (e.g. thetrocar) installed at an incision in the body of a patient, e.g. in theabdominal wall. According to the invention, the method comprises thefollowing steps:

-   determining a position of the instrument relative to the fulcrum    (which in the present context especially means continuously updating    the insertion depth of the instrument or the distance between the    (reference frame of the) sensor and the fulcrum);-   measuring by means of the 6 DOF force/torque sensor a force and a    torque exerted onto the effector unit by the first end of the    instrument; and-   calculating by means of the principle of superposition of a force    exerted onto the second end of the instrument based on the    determined position, the measured force and the measured torque.

The system comprises programmable computing device, such as a standardcomputer, a Digital Signal Processor (DSP) or a Field Programmable GateArray (FPGA), programmed to determine the instrument position, toprocess the measurements made by the 6 DOF force/torque sensor and tocalculate the force estimate as set out above.

The method and system enable estimation (which in the present contextespecially means determination of value(s) that may be affected by asmall inaccuracy) of the force exerted onto a tissue or organ of patientby the second end of the instrument, i.e. the instrument tip, which isinvasively introduced into the patient through an access port such as atrocar. Indeed, the latter force is equivalent to the actio of theopposite force estimated by the method (reactio). As will beappreciated, this method further enables a system design, which requiresonly a single sensor unit that includes the 6-DOF force/torque sensorand mounted on the manipulator i.e. outside the patient. Conveniently,the sensor unit is mounted in force transmission between the connectioninterface for the instrument on the effector unit and the extremelink/member of the manipulator that supports the effector unit. In otherwords, the 6-DOF force/torque sensor is arranged for sensing forces andtorques exerted onto the effector unit by the first end (=mounted end)of the instrument.

Hence, the present invention overcomes the well established generalopinion that sensory equipment must be provided at the level of theinstrument tip and/or the trocar in order to achieve accurate forcemeasurements of forces exerted at the instrument tip. It thus eliminatesexpensive dedicated sensory equipment to be provided on the tip of everyinstrument as well as and on the trocar, that would be subject tostringent miniaturization and sterilization constraints. With thepresented method and system, the latter constraints are overcome, whilea surprisingly accurate estimation of the contact force at theinstrument tip can be achieved.

It will be understood that the presented method/system can be used inconnection with a manually operated manipulator (instrument positioningstand) or, more commonly, with a robot manipulator. The method/systemenables among others a facilitated implementation of force-feed back andautomated safety features in tele-operated medical systems, such asminimally invasive robotic surgery and diagnostic systems. For example,tactile sensing on a force-reflecting (haptic) master arm of anoperating console for the surgeon as well as an automated procedure forlimiting the maximum force exerted by the instrument tip onto apatient's organ(s) and tissue(s) can be implemented using informationgained with the present method/system.

In a preferred embodiment, the method comprises determining an initialreference position of the instrument relative to the fulcrum. In thisembodiment, determining the position of the instrument relative to thefulcrum is based on the determined initial reference position and oncontinuous updating using manipulator motion information. This effectiveprocedure takes advantage of known information such as coordinateinformation by direct kinematics of a robot manipulator.

Preferably, the method further comprises the step of calculating bymeans of the principle of superposition an estimate of a force exertedat the fulcrum by the instrument, e.g. onto the trocar, based on thedetermined position, the measured force and the measured torque.Knowledge of the force exerted onto the tissue of a patient at theincision level, of which the force exerted at the fulcrum is the reactio(with opposite sign), allows among others automated (re)adjustment ofthe fulcrum coordinates, which are e.g. used by a robot controller forreducing stresses and loads exerted onto the tissue of the patient atthe incision level. Furthermore, an automated procedure for limiting themaximum force exerted at the access port level can be implemented.

Preferably, the effector unit is further equipped with a 6-DOFaccelerometer. In this case, the method preferably further comprises thesteps:

-   measuring by means of the 6-DOF accelerometer a gravity load and    dynamic loads exerted onto the 6-DOF force/torque sensor; and-   compensating the gravity and/or dynamic loads in the measured force    and the measured torque.    Such compensation allows to improve the accuracy of the desired    force estimate(s) at the instrument tip and/or at the fulcrum level.

Advantageously, the method further comprises a calibration procedureincluding the additional steps:

-   passing the effector unit through a set of poses distributed over a    workspace, in particular the orientation workspace, of the    manipulator;-   recording for each pose a measured force and a measured torque; and-   determining force and torque measurement offsets based on the    recorded force and torque measurements.    In a further preferred embodiment, in case the 6-DOF accelerometer    is provided, the calibration procedure further comprises the steps:-   recording for each pose a measured linear acceleration and a    measured angular acceleration; and-   determining linear and angular acceleration measurement offsets    based on the recorded linear and angular acceleration measurements.    The calibration procedure allows determining (electrical) offsets in    the measurement signals provided by the sensors and further useful    system parameters, knowledge of which enables further improvements    in the accuracy of the desired force estimate(s).

For reducing measurement signal noise, the method advantageouslycomprises applying a linear Kalman filter (according to the basic asopposed to e.g. the non-linear extended Kalman formulation) to force andtorque data measured by the 6 DOF force/torque sensor prior tocalculating the estimated force or applying a linear Kalman filter tothe calculated force estimate, i.e. after the estimated force(s) havebeen calculated. Among the many available filter types, the basic linearKalman filter has been found to be a simple, reliable and fast filterfor removing signal noise in the measured components.

In case the accelerometer is provided, the method may preferablycomprise the steps:

-   applying a primary linear Kalman filter to force and torque data    measured by the 6 DOF force/torque sensor and to linear and angular    acceleration data measured by the 6 DOF accelerometer;-   compensating disturbances due to gravity and dynamic loads after    application of the primary linear Kalman filter;-   applying a secondary linear Kalman filter to the compensated force    and torque data.    Every Kalman filter for each force/torque and acceleration component    should cause the same filter inherent response-delay. In case there    is excessive noise in the force component estimates after    compensation (due to acceleration signals being noisier than the    force/torque measurements), a secondary filter after disturbance    compensation is preferred. The primary filter reduces noise-induced    falsification during compensation whereas the secondary filter    allows smoothing the compensation results.

Preferably, the Kalman filter, respectively the primary and/or secondaryKalman filter, is cascaded and has a first linear Kalman filter stagewith a process noise covariance parameter set to a higher value,preferably in the range between 0.1 and 1, and a second linear Kalmanfilter stage with a process noise covariance parameter set to a lowervalue, preferably in the range between 0.001 and 0.1. At a givenmeasurement noise covariance, the cascaded filter configuration enableslower total response-delays when compared to a single stage filter for agiven noise reduction capacity.

As will be appreciated, the system is adapted for use with a sensorlessinvasive instrument. It further beneficially comprises a sensorlesstrocar, preferably with a magnetic-based air-valve and especiallywithout plastic cap. Furthermore the system advantageously comprises atrocar without gas tap which is preferably made to the major extent ofplastic material so as to save weight.

The system may comprise a software program stored by the programmablecomputing device, which includes program code for performing all thesteps of any one of the above embodiments of the method when thesoftware program is run on the programmable computing device. Theinvention also concerns a software program product comprising programcode stored on a machine-readable storage medium which, when running onprogrammable computing device or loaded onto a programmable computingdevice, causes the programmable computing device to perform all thesteps of any one of the above embodiments of the method.

While the present patent application as filed in principle concerns theinvention as defined in the claims attached hereto, the person skilledin the art will readily understand that the present patent applicationcontains support for the definition of other inventions, which coulde.g. be claimed as subject matter of amended claims in the presentapplication or as subject matter of claims in divisional and/orcontinuation applications. Such subject matter could be defined by anyfeature or combination of features disclosed herein.

BRIEF DESCRIPTION OF THE DRAWINGS

Further details and advantages of the present invention will be apparentfrom the following detailed description, which is not intended to belimiting, with reference to the attached drawings, wherein:

FIG. 1 is a perspective view of a robot manipulator for a minimallyinvasive medical system according to a preferred embodiment of theinvention;

FIG. 2 is a partial perspective view of a minimally invasive instrument,the tip of which inserted into a patient and the opposite end of whichis mounted to an effector unit of the robot manipulator of FIG. 1 , forillustrating a fulcrum force and a tip force;

FIG. 3 is an enlarged perspective view of the effector unit shown inFIG. 2 , illustrating a reference coordinate frame of a force/torque andacceleration sensor provided on the effector unit;

FIG. 4 is a block schematic diagram of a cascaded linear Kalman filter;

FIG. 5 is a block schematic diagram of a software architecture forperforming the method according to the invention;

FIG. 6 is a state transition diagram of the main task (FSS task) of thearchitecture in FIG. 5 ;

FIG. 7 is a flow chart of a sequence of program steps to be carried outcyclically during the APPLICATION_LOADS_EVALUATION state of FIG. 6 ;

FIG. 8 is a flow chart of an alternative sequence of program steps to becarried out cyclically during the APPLICATION_LOADS_EVALUATION state ofFIG. 6 .

DETAILED DESCRIPTION OF PREFERRED EMBODIMENTS System Components andMechanical Configuration

FIG. 1 shows the main mechanical components of the minimally invasivemedical system according to the invention. The system comprises a robotmanipulator, generally identified by reference numeral 10. An effectorunit 12 is connected to a flange of the manipulator 10. A minimallyinvasive instrument 14, is mounted with a first end 16 to the effectorunit as shown in FIG. 1 . The instrument 14 comprises an elongated shaft18 with a tip 20 forming the second end of the instrument 14. At its tip20, the instrument 14 normally comprises a specific tool e.g. grasper,scissor, hook, coagulator, etc. The robot manipulator 10 itself provides6 degrees of freedom (DOF) by means of a PRP-RRR joint arrangement forpositioning and orienting the effector unit 12, the effector unit 12being mounted to the foremost rotational (R) joint for rotating theminimally invasive instrument 14 about the 6^(th) DOF of the manipulator10 which coincides with the longitudinal shaft axis of the instrument14. As will be appreciated, the robot manipulator 10 provides a 6 axispositioning and orienting device capable of replicating the motion of asurgeon's hand by moving the effector unit 12.

FIG. 2 shows the instrument 14, mounted to the effector unit 12 of therobot manipulator 10, in operational position for performing a minimallyinvasive medical procedure. As indicated by a dashed line in FIG. 2 ,the shaft 18 of the instrument 12 is partially inserted into a patient'sbody, e.g. into the abdomen of a patient. The instrument slideablypenetrates through an access port, referred to as trocar 22 hereinafter.The first end of the instrument 14, i.e. the tip 20 is located beyond afulcrum, indicated by cross-shaped broken lines at 23, (also calledpivot point) defined by the trocar 22 which is inserted into an incisionin the patient's abdominal wall and fixed thereto.

In normal use, the fulcrum is a kinematic constraint that allowsrotation around three axes (e.g. two orthogonal pivot directions and onerotation about the instrument axis, i.e. the Z axis in the SRF definedbelow) but translation of the instrument 14 only along the penetrationaxis (e.g. of the trocar 22—Z in the SRF defined below). The fulcrum isdefined by the access port, e.g. by the trocar 22, and/or the tissue ofthe patient in which the incision is provided, e.g. the patient'sabdominal wall.

FIG. 2 schematically indicates two forces F_(Fulcrum) and F_(Tip)·F_(Tip) is a force exerted onto the instrument tip 20 and thereforerepresents the reactio corresponding to the (opposite) force (actio)that the instrument tip 20 exerts on an internal organ or tissue of thepatient. F_(Fulcrum) is a force exerted onto the trocar 22 and thereforerepresents the reactio corresponding to the (opposite) force (actio)that the trocar 22, which is subject to loads exerted thereon by theinstrument shaft 18, exerts onto the patient's abdominal wall. Theproposed method for determining both, F_(Tip) and F_(Fulcrum) will bedescribed hereinafter.

Although not shown in the figures, the system further comprises amanipulator controller, i.e. hardware, e.g. in the form of a maincomputer, programmed with software for operating one or more robotmanipulators 10. Furthermore, a command console for tele-operation witha force reflection master arm, i.e. a haptic interface forforce-feedback, is used by an operator, e.g. a surgeon, to command therobot manipulator 10 via the manipulator controller. As will beunderstood, the estimate of F_(Tip) will be fed to the haptic interfacefor providing force-feedback and to the motion controller for safetyfunctions. The motion controller also uses the estimate of F_(Fulcrum)for safety functions and for readjusting the assumed coordinates of thefulcrum 23.

FIG. 3 shows an enlarged view of the effector unit 12 which is arrangedto support the first end 16 of the instrument 14 (not shown in FIG. 3 )in mechanically rigid manner and further provided with actuating meansfor actuating certain types of instruments and signal and powerconnection means for electrically connecting the instrument 14 to thesystem. The effector unit 12 comprises a rigid main body 24 includingthe actuating and connection means as well as a socket 26 to which anadapter at the first end 16 of the instrument 14 (not shown) can berigidly connected. At its rear end, the main body 24 comprises aconnection flange 28 by means of which it is rigidly fixed to thesensing plate of a 12-DOF (i.e. 12 axis) force/torque and accelerationsensor 30, referred to as F/TAS 30 hereinafter. The F/TAS 30 may beconfigured as single sensor unit comprising a 6-DOF force/torque sensor,referred to as F/T sensor hereinafter, for sensing forces and torques onthree orthogonal axes, and a built-in 6-DOF accelerometer, for sensinglinear and angular acceleration about the three orthogonal axes.Alternatively, a 6-DOF force/torque sensor with an appropriatelyassociated separate 6-DOF accelerometer can also be used. The F/TAS 30in turn is rigidly fixed to the robot manipulator 10, as seen in FIG. 1. Instead of the described F/TAS 30, a sensor unit comprising only a6-DOF F/T sensor (i.e. no accelerometer) can be used. In the lattercase, acceleration components can be determined using the secondderivative of position coordinates of the end-effector (e.g. effectorunit 12) obtained e.g. by direct kinematic computation usingarticulation positions. Compensation of dynamic loads as describedhereinafter can thus be achieved without accelerometer. It may be notedthat the effect of gravity can also be compensated without accelerometersince the gravity vector is known and the orientation and center ofgravity of the payload attached to the F/T sensor can be determined.

FIG. 3 further shows the Cartesian reference coordinate frame of theF/TAS 30, with the three orthogonal axes X, Y and Z, hereinafterreferred to as SRF (sensor reference frame). As will be understood, the6 DOF of the F/T sensor in the F/TAS 30 correspond to 3 DOF for X, Y andZ force components respectively and 3DOF for moments (torque values)about the X, Y and Z axes respectively, in the SRF. In case a separate6-DOF accelerometer is attached to a 6-DOF F/T sensor for providing theF/TAS 30, the reference coordinate frame of the accelerometer ispreferably coincident with the reference coordinate frame of the F/Tsensor. Otherwise, an additional transformation between these twoCartesian frames shall be added in the calculations describedhereinafter. In the embodiment shown in FIGS. 1-3 , the 12 axis F/TAS 30comprises a built in 6-DOF accelerometer. The 6 DOF of the accelerometercorrespond to linear acceleration components along and angularacceleration components about the X, Y and Z axes respectively, in theSRF shown in FIG. 3 .

As will be understood, the effector unit 12 is rigidly fixed to thesensing plate of F/TAS 30 and preferably configured such that thelongitudinal (shaft) axis of a mounted instrument 14 (cf. FIG. 2 ) iscollinear with one axis of the SRF of the F/TAS 30, preferably the Zaxis as seen in FIG. 3 . Otherwise, an additional transformation shallbe added in the calculations described hereinafter.

Main Disturbance Sources and Analysis Thereof

The present section gives an overview of main disturbance sources thataffect the desired estimation of the force at the instrument tip 20,with the system presented in FIGS. 1-3 .

Besides the intrinsic F/T sensor disturbances such as sensor offsets,electrical noise and temperature drifts, with the present system thereare, as opposed to other known force sensing systems (e.g. using a F/Tsensor mounted on the instrument tip), a number of additional disturbingand masking factors to be taken into account. As regards measured forceand moment information, these are mainly:

-   static and dynamic loads exerted onto the F/T sensor: static loads    due to gravity (weight of the mass attached to the manipulator    mounted F/TAS 30), dynamic loads due to the velocity and    acceleration of the payload attached to the F/T sensor;-   disturbance sources related to the minimally invasive medical    procedure: trocar friction forces in the penetration and extraction    direction due to the trocar gas tap and the air valve, resistance to    pivot due to the trocar gas tap, the modification of the fulcrum 23    (pivot point) due to the variations of abdominal insufflation    pressure, inaccurate definition of the fulcrum 23, modification of    the fulcrum 23 due to inaccuracy of the manipulator 10 while moving.

Disturbing forces produced by the trocar friction: The trocar 22produces friction along the penetration/extraction axis. The frictionmagnitude depends on the type of air-valve used in the trocar 22 (e.g.magnetic, spring-based or plastic membrane type), on the plastic capwear, on the material of the instrument shaft 18 and on its internallubrication by irrigation water and viscous intra-abdominal fluids.According to laboratory trials, friction caused by magnetic andspring-based air-valves can be approximated by a Coulomb friction in arange of 0.5N-0.9N and does not depend on lubrication conditions. Inpractice, the spring-based air-valve friction depends slightly on itswear, and is higher than magnetic air-valves friction by approximately0.3 N. The plastic membrane air-valve and the plastic cap produce aCoulomb friction but also an impulse-like reaction force when invertingthe instrument direction. This reaction component is opposed to themotion direction and is mainly caused by the plastic collar reversal.The membrane and cap friction depends on the membrane cut geometry andon the type of material, but is attenuated by lubrication of the trocar22 which increases along with the intervention time through instrumentsmoves. In dry laboratory trials using standard trocars, plastic capsproduce a Coulomb friction in the range of 1N-1.5N, and plastic membraneair-valves give a Coulomb friction in the range of 6N-10N. In addition,the friction magnitude is found to be asymmetric with respect to thepenetration and extraction directions. For plastic membrane valves,smaller friction amplitude was observed in the penetration direction.Therefore, in order to reduce the penetration and extraction friction atthe trocar 22 as much as possible, magnetic-based air-valves, possiblywithout plastic cap, are preferred

Disturbing forces produced by trocar gas tap: Some types of trocars havea tap for insufflating gas. The tap and the connected gas tube can actas obstacles when pivoting the trocar 22, resulting in a disturbingresistance force opposed to the pivot direction. The magnitude of thisforce depends on the stiffness of the abdominal wall and is generallybetween 2N and 5N according to laboratory trials. Hence, use of trocarswith gas tap should be avoided with the presented system.

Disturbing force produced by the trocar weight: Multiple-use trocars areusually lightweight, from 30 g to 80 g, and made of stainless steelpossibly with some parts made of plastic. Trocars with a gas tap have acylindrical reservoir and are heavier, ranging from 100 g to 180 g. Thetrocar weight can be perceived as a disturbing force along thetransversal X and Y axes in the SRF, depending on the orientation of thetrocar 22 with respect to the gravity vector. Therefore, lightweighttrocars made with plastic parts are preferred with the proposed system.

Disturbing forces produced by low intra-abdominal pressure: In nominallaparoscopy conditions, the abdominal wall is a relatively stiff surfaceto which the trocar 22 is attached. In case of low intra-abdominalpressure, the trocar friction magnitude may become higher than theresistance offered by the abdominal wall. In this case, instrumentpenetration or extraction can move the trocar 22 inwards or outwards upto the point where the abdominal wall tension overcomes the trocarfriction. Negative side-effects are firstly, that the location of thefulcrum 23 is altered with respect to the abdominal wall, wherebydisturbing loads during pivoting increase due to the interaction of theinstrument with the abdominal wall, and secondly, a spring-like load(with a maximum value equal to the trocar friction) is applied in thedirection opposite to the instrument motion. In order to avoid thesedisturbing forces, the intra-abdominal pressure is preferablycontinuously monitored and maintained. In case of depressurization, awarning is issued in order to take appropriate actions, such asadjusting fulcrum position in the manipulator controller.

Disturbing forces from inaccuracies in the determination of the fulcrumlocation: In manual laparoscopic surgery, the surgeon naturally movesthe instrument with respect to the minor tilting resistance point, whichis the ideal fulcrum 23 (pivot point), located at about the height ofthe stiffest layer of the abdominal wall, inside the trocar 22. Whenusing a robot manipulator 10 for handling the instrument 14, without anyspecifically designed mechanical compliance as regards the fulcrum 23,the fulcrum position should be determined by a suitable procedure andtaught to the manipulator controller. In case the fulcrum position isinaccurately defined, pivoting of the instrument 14 generatesinteraction forces with the abdominal wall that can mask the desiredforce/torque values at the instrument tip 20 and/or the fulcrum 23.These masking forces increase with the magnitude of the fulcrum positioninaccuracy. In addition, such inaccuracy produces wear on the incision,which can lead to the release of the trocar 22, in turn provoking lossof abdominal pressure and thereby unnecessarily increasing theintervention time due to the required recovery of the situation.

The definition accuracy of the position of the fulcrum 23 depends notonly on the procedure used to identify its position it but also on thestatic and dynamic accuracy of the robot manipulator 10. In the presentapplication, a +/−2.5 mm estimate of overall fulcrum and manipulatoraccuracy could be acceptable considering the incision dimension and theelasticity of the abdominal wall. According to an experimental set-up,definition inaccuracies regarding the fulcrum 23 may lead todisturbances of 2N-10N at the level of the trocar 22.

As a result, an appropriate selection of the type of trocar 22 permitsto avoid the gas tap disturbance and to reduce friction and weightdisturbances along the axis of instrument shaft 18 to the level oftypical human hand sensitiveness which is around 0.6N. Real-timemonitoring of intra-abdominal pressure variations with respect to thepressure at initial fulcrum definition, can detect a variation of thetrue fulcrum location due to varying insufflation conditions. However,the disturbance force at the access port level (i.e. fulcrum 23 or pivotpoint), due to an inaccurate definition of the fulcrum 23 and due tomotion inaccuracy of the manipulator 10, can be identified in real-timethrough the proposed method described hereinafter.

The proposed method and system are able to overcome the encountereddisturbance issues, thereby enabling tele-operation with accurate forcefeed-back and a number of other beneficial safety-related functionsbased on force information, obtained exclusively from a sensorarrangement mounted onto the manipulator 10, i.e. outside the patient.There is no need for further sensors, neither on the instrument 14 noron the trocar 22.

Calculating Forces at the Instrument Tip and at the Fulcrum Level

The proposed method permits to provide an accurate estimate of theforces F_(Tip) at the instrument tip 20 and F_(Fulcrum) at the fulcrum23

An main point of this method is the calculation of the forces F_(Tip)and F_(Fulcrum) , using the force and torque components measured by theF/TAS 30 which, as will be understood is located at a remote point withrespect to the respective points of application of F_(Tip) andF_(Fulcrum) . This calculation furthermore uses a determined position ofthe instrument 14 relative to the trocar 22, e.g. the distance betweenthe fulcrum 23 and the origin of the SRF of the F/TAS 30 shown in FIG. 3. This calculation is based on several assumptions and pre-requisites,as follows:

A. The 6-DOF F/T sensor in the F/TAS 30 measures the three components offorces (Fx, Fy, Fz) and the 3 components of moments (Mx, My, Mz)produced by the load attached to the F/TAS 30 in a right-hand Cartesianframe as shown in FIG. 3 (SRF).

B. The instrument 14 is attached to the F/T sensor through a support,that can contain one or more actuators for the instrument mechanism aswell as further other subsystems (i.e. the effector unit 12).

C. For purposes of ease of description, it is assumed that the effectivereference frames of the 6-DOF F/T sensor and the the 6-DOF accelerometerof the F/TAS 30 coincide with the SRF shown in FIG. 3 in which the Zaxis is collinear with the longitudinal axis of a mounted instrument 14and points towards the instrument tip 20, the Y axis is parallel to theupper surface of the main body 24 and the origin is located on thesensing plate of the F/TAS 30. In case the forces and torques measuredby the F/T sensor are expressed with respect to another frame, atransformation can be applied to express the measured forces and momentvalues with respect to the SRF.

D. The values of force and torque components used in the equationshereinafter are obtained from originally unfiltered 6-DOF F/T sensormeasurements after subjecting the latter to compensation of electricaloffsets, gravity and acceleration loads and a specific filtering processfor reducing measurement noise as described hereinafter.

E. Only two external contact forces are applied to the instrument 14 asshown in FIG. 2 , i.e. the reaction force at the fulcrum 23 (F_(Fulcrum)), which is assumed to be tangent to the abdominal wall, and a contactforce (F_(Tip) ) on the instrument tip 20 which may have any directionand sense.

F. The fulcrum reactio expressed in the SRF, noted F_(Fulcrum) , has anull Z component and there are no external moments applied to thefulcrum 23.

G. The external force applied to the instrument tip 20 is expressed inthe SRF and noted F_(Tip) ·F_(Tip) equals the opposite of the forceexerted onto the tissue/organ contacting the instrument tip(actio+reactio=0). There are no external moments applied to theinstrument tip 20.

H. The distance vector D_(Fulcrum) from the origin of the SRF to thefulcrum 23 is known and has a component along the Z axis only. Inpractice there may be X and Y components of a few millimeters if theshaft 18 of the instrument 14 is bent and therefore the distance alongthe Z axis may be slightly inaccurate. This distance vector D_(Fulcrum)can be determined, i.e. continuously updated from an initial reference,using procedures outlined hereinafter.

I. The distance vector D_(Tip) from the origin of the SRF to theinstrument tip 20 is known and is aligned along the Z axis.

Taking into account the above assumptions, the resulting torque andmoment in the SRF, respectively noted T_(S) and F_(S) , can becalculated using the principle of superposition applied to forces andmoments by means of the following equations:

T _(S) = F _(Tip) × D _(Tool) + F _(Fulcrum) × D _(Fulcrum)   (10)

F _(S) = F _(Tip) + F _(Fulcrum)   (11)

Where D_(Tool) represents the vector from the origin of the SRF to theinstrument tip 20, which is collinear with the Z axis of the SRF.

Contact force components at the Instrument-tip 20 are determined bysubstituting F_(Fulcrum) in (10), which results in:

$\begin{matrix}{{F_{Tip}(x)} = \frac{{T_{S}(y)} - {{F_{S}(x)} \star {D_{Fulcrum}(z)}}}{{D_{Tip}(z)} - {D_{Fulcrum}(z)}}} & (12)\end{matrix}$ $\begin{matrix}{{F_{Tip}(y)} = \frac{{T_{S}(x)} + {{F_{S}(y)} \star {D_{Fulcrum}(z)}}}{{D_{Fulcrum}(z)} - {D_{Tip}(z)}}} & (13)\end{matrix}$ $\begin{matrix}{{F_{Tip}(z)} = {F_{S}(z)}} & (14)\end{matrix}$

Similarly, force components at the fulcrum 23 are:

$\begin{matrix}{{F_{Fulcrum}(x)} = \frac{{T_{S}(y)} - {{F_{S}(x)} \star {D_{Tip}(z)}}}{{D_{Fulcrum}(z)} - {D_{Tip}(z)}}} & (15)\end{matrix}$ $\begin{matrix}{{F_{Fulcrum}(y)} = \frac{{T_{S}(x)} + {{F_{S}(y)} \star {D_{Tip}(z)}}}{{D_{Tip}(z)} - {D_{Fulcrum}(z)}}} & (16)\end{matrix}$

As will be appreciated, an accurate estimation of the contact forcesF_(Tip) and F_(Fulcrum) applied at the instrument tip 20 and at thefulcrum 23 respectively, allows, among others, improvements in safetyand quality of robotically assisted minimally invasive medicalprocedures. For instance, the assumed location of the fulcrum 23 withrespect to which the robot manipulator 10 is moved , can be continuouslyadjusted by the robot control software, in real-time during theprocedure, towards a point of minimum resistance using F_(Fulcrum) .Furthermore, the contact forces at the instrument tip 20 can bereflected by the (master) arm with which the surgeon commands the(slave) robot manipulator 10, so as to enable tactile sensing.

Determining the Instrument Position Relative to the Fulcrum

An initial reference position of the instrument relative to the fulcrum,e.g. distance D_(Fulcrum 0) can be determined through the procedure setout below, when a given instrument 14 is inserted for the first time inthe trocar 22. Using the initial reference distance D_(Fulcrum 0) ,D_(Fulcrum) is subsequently continuously updated (i.e. determined inreal-time) using the commanded penetration/extraction, which is afunction of the motion of the manipulator, which in turn is known fromthe manipulator controller.

An example of the procedure to determine the initial fulcrum position(reference distance D_(Fulcrum 0) ) is based on the assumption that thefulcrum 23 is the point of minor force resistance and can be found usingthe F/T sensor on the effector unit 12. For this procedure, it isassumed that the X and Y axes of the SFR lie in the front plane of thesensing plate of the F/T sensor while the Z component is collinear withthe instrument shaft 18. The procedure is outlined as follows:

Step 1—Insertion of the instrument 14, that is attached to themanipulator 10, into the trocar 22, until the instrument tip 20 is seenon the endoscope monitor (i.e. exiting the trocar sleeve).

Step 2—Determination of the position of the instrument 14 that gives thelowest reaction forces along the X and Y axes of the SRF, by sliding theinstrument 14 along these axes until reaction forces are below a giventhreshold, e.g. of 0.3N. Once a suitable point is found, it can beassumed that the fulcrum 23 is located at a certain point along theinstrument axis, i.e. on the Z axis.

Step 3—Determination of the position of the fulcrum 23 (Z axiscoordinate) on the instrument axis (which corresponds to the Z axis)using the lever principle, where the distance at which the force isapplied is equal to the module of the moment vector divided by themodule of the force vector.

Since at step 2, the instrument position corresponds to a near-zerocontact force (F_(Fulcrum) ), the instrument 14 is pivoted with respectto its tip 20 until a sufficient contact force (about 3N) is reached. Atthis point the distance is computed according to the lever principle.Subsequently, the instrument is pivoted in the opposite direction untilthe same contact force value is measured and the again the distance iscomputed again. Thereafter, the instrument 14 is pivoted to its initialposition determined in step 2. The reference distance D_(Fulcrum 0)between the fulcrum 23 and the origin of the SFR on sensor (along the Zaxis) is set to the mean value of the last two measurements.

As both, the position and orientation of the SRF in the world referenceframe and the initial reference distance D_(Fulcrum 0) , giving theposition of the fulcrum 23 with respect to the SRF (i.e. sensor) restingat the location found in step 2,the fulcrum location with respect to theworld reference frame can be computed through a simple change ofreference frame (transformation of coordinates).

Afterwards, all moves (pivot and penetration) can be given with respectto the fulcrum 23, and the instrument position relative to the fulcrum23, e.g. the distance between the origin of the SRF and the fulcrum 23,can be updated accordingly e.g. using position information from themanipulator controller.

Compensation of Offsets and of Gravity and Dynamics Loads

As will be understood, the force/torque sensor, e.g. in the F/TAS 30,attached to the robot manipulator 10, measures not only the contactforces F_(Tip) , F_(Fulcrum) but also the gravity load as well asdynamic (i.e. motion-related) loads exerted onto the components attachedto the sensing plate of the sensor.

Therefore, the method of force estimation provides for compensations ofthese loads using additional measurements obtained from the 6-DOFaccelerometer associated to the 6-DOF F/T sensor.

The compensated force vector F_(comp) with respect to the sensorreference frame (SRF) is given by:

F _(comp) = F _(sensor) − F _(offsets) −LoadMass·((LinAcc_(sensor)−LinAcc_(offsets) ) +((AngAcc_(sensor) −AngAcc_(offsets) )×Load_(COG)))  (17)

where:

-   -   F_(sensor) is the force vector in the SRF as measured by the F/T        sensor;    -   LinAcc_(sensor) is the linear acceleration, including the        gravity acceleration, measured by the 6-DOF accelerometer in the        SRF;    -   AngAcc_(sensor) is the angular acceleration measured by the        6-DOF accelerometer in the SRF;    -   Load_(COG) is the vector of the center of gravity of the load        attached to the 6-DOF F/T sensor in the SRF, that is estimated        as outlined hereinafter;    -   F_(offsets) , LinAcc_(offsets) and AngAcc_(offsets) are vectors        of sensor offsets, that are estimated during a calibration        procedure outlined hereinafter;

The compensated torque vector T_(comp) with respect to the sensorreference frame (SRF) is given by:

T _(comp) = T _(sensor) − T _(Offset) −((Load_(COG) × F _(T))+LoadInertia·(AngAcc_(sensor) −AngAcc_(offiets) ))  (18)

where:

-   -   T_(sensor) is the moment vector in the SRF as measured by the        F/T sensor;    -   T_(Offset) is the moment offset vector, estimated as outlined        hereinafter;    -   F_(T) equals the third term on the right-hand side of (17) which        represents the force produced by the effect of gravity and of        acceleration-related loads, which exerts a torque onto the        sensing plate of the F/TAS 30;    -   Loadlnertia is the vector of the load inertia about SRF axes X,        Y and Z, that can e.g. be estimated by visual tuning in an        off-line analysis, i.e. observing the compensation accuracy        improvement on a measurement plot for different values of the        inertia vector.

As regards the effect of Coriolis acceleration, which depends on theangular acceleration and linear velocity of a moving frame with respectto a fixed one, it may be noted that this effect does not need to betaken into account with the present system, because forces and torquesare measured with respect to the moving reference frame of the F/Tsensor (SRF).

The effect of the centrifugal acceleration along the instrument stemaxis, i.e. the Z-axis of the SRF, in the presented system hasempirically been found to be less than 0.2N for typical instrument movesand less that 0.4N for fast moves in minimally invasive procedures.Although mentioned for the sake of completeness, it has beenexperimentally found that this effect can be neglected and is thereforenot taken into account in equations (17) and (18).

For a typical system setup, experimental results in no-contact but fastmoves, i.e. about 60 degrees/second for pitch and yaw pivot DOF and 150mm/sec for the penetration/direction, show that forces are compensatedwithin a +/−0.25N window, and that moments are compensated within a+/−0.03 Nm window approximately.

As will be understood, the compensated force and torque vector will beused for the calculation described in section “Calculating forces at theinstrument tip and at the fulcrum level”, i.e. F_(comp) =F_(s) andT_(comp) =T_(s) .

Calibration Procedure

In order to determine system related parameters that affect measurementaccuracy and calculations for force estimation, a suitable fittingtechnique, e.g. a least-squares fitting method, is applied on a seriesof measured data. In order to obtain data series for applying theleast-square fitting technique, the robot manipulator 10 isconsecutively positioned through a suitably predefined set ofmeasurement poses distributed over the workspace of the robotmanipulator 10. At each pose, corresponding to a different position andorientation of the F/TAS 30 through different configurations of the 6DOF of the manipulator 10, the robot manipulator 10 is at rest whenmeasurement data is read from the sensors of the F/TAS 30. The set ofposes is preferably chosen so as to cover a sufficient range(“orientation workspace”) of the following orientation angles: rotationabout the Z-axis of the SRF (“roll”) and either rotation about the pitchor the yaw pivot axis (for instance using a wrist articulation/jointthat varies the sensor orientation with respect to gravity).

If appropriately chosen, it is safe to assume that the F/TAS 30 isfactory-calibrated and that the accuracy and the resolution of thesensor are far beyond the application requirements. In this case, thefitting technique applied to the measurement data series enables amongothers accurate identification of (electrical) offsets of force andtorque component measurements on each axis as well as (electrical)offsets of linear acceleration component measurement on each axis.Furthermore, the mass LoadMass and centre of gravity (COG) of the loadattached to the sensing plate of the F/TAS 30 can be accuratelydetermined using the calibration procedure as described below.

For the determination of force measurement offsets (F_(offsets) ), theeffective load mass (LoadMass), and the linear acceleration offsets(LinAcc_(offset) ), the following equation is used:

F _(sensor) = F _(offsets) +LoadMass*(LinAcc_(sensor) −LinAcc_(offset))  (21)

where:

-   -   F_(sensor) is the force vector, as measured by the F/T sensor,        in the SRF;    -   (LinAcc_(sensor) −LinAcc_(offset) ) gives the orientation of the        gravity force with respect the SRF, since the linear        acceleration measurement (LinAcc_(sensor) ) comprises the        gravity acceleration term in addition to the motion-related        acceleration (=null at rest) and an electrical offset        (LinAcc_(offset) )    -   LoadMass*(LinAcc_(sensor) −LinAcc_(offset) ) is the weight force        vector given by the mass of the payload attached to the F/TAS 30        and by its orientation, with respect to the SRF

For the determination of moment measurement offsets (T_(offsets) ) andof the coordinates of the centre of gravity of the payload with respectto the SRF (Load_(COG) ), the following equation is used:

T _(sensor) =Load_(COG) ×LoadMass*(LinAcc_(sensor) −LinAcc_(offset) )+ T_(offsets)   (22)

where (LoadMass, LinAcc_(offset) ) are as indicated above, see (21). Forthe determination of the linear acceleration measurement offsets, theequation is:

MODULUS(LinAcc_(sensor) −LinAcc_(offset) )=1G  (23)

where:

-   -   G is the gravity constant.

As will be understood, vector equations (21), (22) and (23) provide 7scalar equations with 13 unknowns for every measurement of the F/TASsensor in a given calibration pose of the manipulator 10.

Since the robot manipulator 10 and hence the F/TAS 30 is at rest in eachpose, i.e. there is no motion when the measurements are taken, theoffsets of the angular acceleration components can be estimated based ona mean value of angular acceleration measurements for all poses:

MEAN(AngAcc_(sensor) )=AngAcc_(offset)   (24)

Where:

-   -   AngAcc_(sensor) is the angular acceleration vector measured by        the accelerometer;    -   AngAcc_(offset) is the electrical offset vector for the angular        acceleration component

The pose set shall be selected to cover the orientation workspace of themanipulator 10 in the surgical application. For instance, such anorientation workspace shall sample the roll angle about the Z axis ofthe SRF and the orientation angle given by the Z axis of the SRF withrespect to the gravity axis. Experimentally, a number of 30 poses,corresponding to 210 equations, has generally been found sufficient fora satisfactory approximation of the required system parameters.

Since electrical offsets can differ at every start-up, the calibrationprocedure should be executed at start-up before using any measurementsfrom the F/TAS 30. As described in section “Check of offset drifts”, itmay be advantageous to repeat the calibration procedure also during anintervention in order to take into account offset drifts. In this case,the system needs to drive the manipulator 10 through the set of poses,which has to be done in safe conditions.

An interesting aspect of this calibration method is that there is noneed for knowledge of the position and orientation of the end-effector(e.g. effector unit 12), which also means that this method isindependent of the robot manipulator accuracy. Therefore, forapplications where compensated forces have to be measured, e.g. onhand-held portable devices, a simple manually actuated, i.e. passive,positioning device can be subjected to the present calibrationprocedure.

As will be understood, the above calibration procedure with subsequentapproximation (data fitting method) allows among others to determineF_(offsets) , T_(offsets) , LinAcc_(offsets) and AngAcc_(offsets) , usedin equations (17) and (18) for compensation of offsets in the sensordata obtained from the F/TAS 30.

Sensor Data Filtering

A filtering technique should be applied to the raw measurement dataobtained by means of the F/TAS 30. Although in principle many suitabletechniques exist, the application of the basic classical form and of twovariants of the discrete Kalman filter for linear stochastic processesis proposed in order to efficiently estimate acceleration andforce/torque process variables, and in particular to reduce measurementnoise inherent to the F/T sensor and accelerometer.

In a minimally invasive medical application using robotic tele-operationwith force-feedback, apart from removing the signal noise to asatisfactory extent, it is highly desirable that the used filteringprocess complies with two additional requirements: firstly, theamplitude gain of filtered signals should be close to 1 (in the systembandwidth) in order to ensure force feedback fidelity and, secondly, theadditional time delay that is introduced by the filter should be asshort as possible. Preferably, the total tele-operation cycle delay,including the signal filtering delay should be less than 100milliseconds in order that the surgeon does not visually notice a delay,e.g. in case of an instrument to tissue contact. Moreover, in order toavoid instability, e.g. when touching hard surfaces such as bones withthe instrument tip 20, the total tele-operation cycle delay shallpreferably be less than 20 milliseconds.

It has been experimentally found that a basic (digital) linear Kalmanfilter is a simple and efficient solution. Among others, it providesbetter noise rejection and dynamic behavior than some other filtertypes, in particular when compared to classical Tchebyscheff digitalfilters commonly implemented in the firmware of commercial force/torquesensors. As opposed to an extended Kalman filter type for the force andtorque data processing, the present approach is applicable in real-time,is more easily tuned and avoids the need for knowledge of the non-lineardynamic model of the robot manipulator 10 which is difficult to identifyprecisely.

Since the aim of the filter is to estimate noisy digital signals whichare measured separately and are not inter-correlated, an instance of thefilter is applied individually to each of the following signalcomponents:

-   Fx, Fy, and Fz for force measurements;-   Mx, My and Mz for moment measurements;-   Ax, Ay and Az for linear accelerations measurements;-   Rx, Ry and Rz for angular accelerations measurements.

According to the basic Kalman filter, every signal can be assumed to bea process governed by a linear difference equation:

x _(k) =Ax _(k−1) +Bu _(k−1) +w _(k−1)

with a measurement z∈

¹ that is:

Z _(k) =Hx _(k) +v _(k)

In the present system we can assume for all signals that H=1 because themeasurement is taken of the state directly and u=0, since there is nocontrol input. Furthermore we assume for all signals: A=1 because thestate is approximated to be invariant from step to step. However, in thecase of forces and moments, the state varies according to gravity andacceleration loads, and for all other signals, the state is function ofthe operator motion commands, i.e. the behavior of manipulator 10.Therefore, this latter approximation assimilates the sources of statevariations to process noise.

As will be appreciated, the proposed filter formulation is that of thebasic discrete Kalman filter implementation which applies to linearstochastic processes. The related time update and measurement updateequations of this filter implementation can be found e.g. in “Anintroduction to the Kalman Filter”; Greg Welch, Gary Bishop; UNC-ChapelHill; 2002, as follows:

$\begin{matrix} & {K_{k} = {P_{k}^{-}{H^{T}\left( {{{HP}_{k}^{-}H^{T}} + R} \right)}^{- 1}}} \\{{\hat{x}}_{k}^{-} = {{A{\hat{x}}_{k - 1}} + {Bu}_{k - 1}}} & {{\hat{x}}_{k} = {{\hat{x}}_{k}^{-} + {K_{k}\left( {z_{k} - {H{\hat{x}}_{k}^{-}}} \right)}}} \\{P_{k}^{-} = {{{AP}_{k - 1}A^{T}} + Q}} & {P_{k} = {\left( {I - {K_{k}H}} \right)P_{k}^{-}}} \\{{time}{update}{equations}} & {{measurement}{update}{equations}}\end{matrix}$

As regards initialization, the following initialization parameters canbe used for all signals:

-   covariance of the measurement noise R=1.0: although the best value    is the real measurement noise covariance that could be obtained in a    sensor calibration phase, any strictly positive value (R>0), meaning    that the measurement is not trusted, can be used. In fact, the    system/process noise covariance parameter Q determined during the    filter tuning phase compensates for errors in the initial    measurement noise covariance value R;-   initial state value x_(k−1)=first observation;-   initial Kalman gain value K_(k)=1.0;-   initial system process/system noise covariance Q₀ determined by    filter tuning.

It has been shown that the Kalman gain K_(k) converges to the sameconstant value independently from the given parameters process/systemnoise covariance Q and measurement noise covariance R, usually after 50cycles of the recursive iteration. With the present system, it has beenfound experimentally that after 150 msec (50 cycles), the Kalman gainK_(k) converges towards the constant value, it remains constant after4.5 sec (1500 cycles) and reaches the 99% window of its constant valueafter 2.1 sec (700 cycles). It has further been found that the Kalmangain K_(k) remains constant irrespective of the (amplitude) of dynamicand contact loads affecting force and torque measurements, whichvalidates the approach of a basic linear filter formulation.

As regards filter (parameter) tuning, an approach based on comparing theunfiltered signal with the filtered signal on the same real-time plotfor different values of system/process noise covariance Q and in realtele-operation conditions (e.g. at 1:1 motion scale, with acceleratedmoves of the manipulator 10 but without contact forces exerted onto theinstrument 14) can be used.

The general purpose of tuning is to obtain a filtered signal withoutspikes or high frequency ripple, that averages the unfiltered signal butwith little response delay on signal transitions (time-lag). In thepresent context, response delay means the filter inherent time-lagbetween the filtered signal and the “true” unfiltered signal observedduring signal variations. For force, torque and acceleration signalswhich are used in the compensation process (see chapter “ Compensationof offsets and of gravity and dynamics loads in sensor data”), allsignals should be filtered with the same covariance parameters R, Q inorder to maintain an identical time-delay behavior for each signal,especially as regards signal transitions. Experimentally, this approachproves to be consistent and can be justified by the fact that the samephysical phenomenon, i.e. motion acceleration of the manipulator 10,nearly exclusively determines the dynamic behavior of the measuredsignals.

As regards a qualitative analysis, it has been demonstrated that, forstatic signals affected by noise, the Kalman filter is as optimalestimator with 1:1 gain. For dynamic signals, as in the present system,the Kalman filtered signal does not have spikes due to noise because thenoise is almost entirely removed, and the filtered signal has similarityto an averaged signal with transitions smoothness depending on thechosen process noise covariance parameter Q.

It will be understood that, with a smaller process noise covariance Q,the filtered signal becomes smoother because the measurement is lesstrusted, and vice versa. Furthermore, with lower values of theprocess/system noise covariance Q set in the Kalman filter, not only thesmoothness of the filtered signal but also the response delay caused bythe filtering process increases for a given measurement noise covarianceR. It is however desirable to have both an immediate and a smoothlyvarying force estimate, e.g. for feed-back to the master arm of atele-operation command console. Table 1 shows typical response delaysfor different process noise covariance parameters Q of a force signal(e.g. on the X-axis of the SRF).

TABLE 1 X-axis Force signal filtered with Kalman during tele-operationProcess covariance Response delay Response delay parameter Q inintervals in ms 1 0.4 1.172 0.1 3 8.79 0.01 11 32.23 0.001 25 73.250.0001 40 117.2

The response delays indicated in Table 1 were evaluated off-line, withmeasurement noise covariance R=1.0, by measuring the time-lag betweenthe filtered signal obtained with the basic linear Kalman filter and thesignal obtained using a parallel backward recursion (RTS) form of theKalman algorithm, as described in “Maximum likelihood estimates oflinear dynamic systems”; H. Rauch, F. Tung, and C. Striebel; AmericanInstitute of Aeronautics and Astronautics Journal; 3(8), 1965, whichoptimally follows the original “true” signal without introducingresponse-delay.

In order to reduce the filter inherent response delay, the cascadedfilter implementation 40 as shown in FIG. 4 is proposed. This filtercascade 40 comprises a first filter stage 42 and a second filter stage44, each filter stage 42, 44 being a separate implementation of a basiclinear Kalman filter as described above. The first filter stage 42 isconfigured to lower the covariance i.e. to reduce the peaks (noisespikes) of the noise affecting the unfiltered signal but to cause only arelatively short response delay (e.g. 2-3 ms). The second filter stage44, is configured to provide a substantially smooth output signal andtherefore introduces a longer response delay (e.g. 15 ms) than the firstfilter stage 42.

It has been found that, for a given total response delay, two cascadedfilters improve the smoothness of the filtered signal with respect to asingle filter causing the same response delay. In order to achieve this,e.g. in a two filter cascade as shown in FIG. 4 , the first filter stage42 is configured with a system/process error covariance (Q₁) that issignificantly greater than the system/process error covariance of thesecond filter stage 44 (Q₂) with given identical measurement errorcovariance R. Thereby, the same filtering performance at lower totalresponse delay when compared to a single stage Kalman filter can beachieved. In other words, a Kalman filter cascade with a given totalresponse delay provides better filtering performance than a single stageKalman filter with the same response delay. By experiment, it has beenfound e.g. that two cascaded Kalman filters, the first and second filterstages 42, 44 being configured with identical measurement noisecovariance R=1 and different system/process error covariance parametersof Q₁=0.7 and Q₂=0.012 respectively, improve the smoothness of the finalfiltered signal with respect to a single stage filter configured withQ=0.01 and producing the same observed response delay (≈32 ms).Preferred parameter ranges for the noise covariance Q1 and Q2 of thefirst and second filter stage 42, 44 respectively are: 0.1 ≤Q₁≤1 and0.001≤Q₂ ≤0.1. Preferably, the total response delay should not exceed 40ms for reducing the risk of instability on hard surfaces contact.

Therefore, a cascade of at least two linear Kalman filters is preferredsince it introduces less response delay with respect to a single-pass(one stage) filter giving the same filtering performance (signalsmoothness). It should be noted that the respective filterimplementation for each unfiltered signal ((Fx, Fy, Fz); (Mx, My, Mz);(Ax, Ay, Az); (Rx, Ry, Rz)) will usually be configured with the samefilter parameters (Q_(i), R_(i), etc.) in order ensure an identicalresponse delay on all signals and, thereby, synchronized signals.

Check of Offset Drifts

As will be understood, every component measurement (signal) obtainedfrom the of F/T sensor and accelerometer in the F/TAS 30 is affected byan electrical bias or offset that is normally time-varying andtemperature dependent. In laboratory trials it has been found thatmeasurement signals from a 6-DOF foil-based F/T sensor (with built-intemperature compensation) stabilize after a warm-up period of about 3hours, and remain thereafter within a range of about 1.5% of the fullmeasurement scale. However, the offset value for each signal is subjectto variation over time and, in case of a medical, especially a surgicalapplication, this variation may be unacceptable, as it alters thecalculation results for estimating the forces as described hereinbefore.

Therefore, it is proposed to include a procedure for checking that theseoffsets remain within an acceptable range. This can be achieved insimple manner by checking whether the compensated force and torquevector F_(comp) , T_(comp) components are near zero when no externalloads are applied on the payload attached to the F/TAS 30. The proposedfunction can consist in a software implemented procedure carrying outthe check upon command request. In case of excessive offset drift, theprocedure sends a warning to the manipulator controller, which shouldfor example ask the surgeon to initiate a re-calibration process.Furthermore, this function can be carried during a surgical instrumentchange, either upon a command given on the HMI or automatically, forexample based on the signal of a surgical instrument presence-detectoron the effector unit 12.

Software Module Architecture

Initially, it may be noted that the software architecture describedhereinafter is refers to a software module whose purpose is limited todata processing and calculations for estimation of contact forces at thelevel of the instrument tip 20 and at the level of the fulcrum 23. Itdoes not take into account functions and mechanisms to the control ofthe manipulator 10, the effector unit 12 or other components of thesystem. This module can however be integrated in the software program ofa manipulator controller by the skilled person.

The general architecture of the software module is schematically shownin FIG. 5 . It comprises a core process, the FSS (force sensing system)task which is governed by a state transition diagram describedhereinafter, that can be implemented in a main function running eitherin task context or at interrupt service routine level. For the sake ofsimplicity, it is assumed that the software module runs in a periodictask synchronized by a real-time clock through a semaphore as shown inFIG. 5 . The FSS task is run at a given priority in the real-timeoperating system and with a given stack size. The software module has amessage queue that is polled at each clock cycle for new messages. Thereare generally two types of messages: command messages to execute afunction or event messages to generate a transition in the statetransition diagram (see below). Command messages are generated byexternal modules pertaining to e.g. the manipulator controller, whereasevent messages are issued internally by the software module itself. Themodule is capable of generating event and command messages directed toother, e.g. manipulator controller modules, for example in order toissue failure events, command replies or stop_motion commands.

In the software module, the main interfaces of the FSS task are, asshown in FIG. 5 :

-   a message queue, read at every clock cycle;-   an interface to hardware boards from which unfiltered force, torque    and acceleration data is read;-   an interface to a real-time data base to read information required    by the functions of the modules and to write results-   an interface for commands and event messages to external modules.

State Transition Diagram (FSS task)

FIG. 6 shows the main five states of the Force Sensing System (FSS) task(cf. FIG. 5 ) implemented as finite state machine. In the following, thestates shown in FIG. 5 will be briefly described:

State 1: Hardware and software initialization: this state concerns theinitialization procedures for the software and hardware parts of theminimally invasive medical system. These initialization procedures arecarried out at power-up and/or at boot time of the controller of themanipulator 10. The hardware initialization task concerns among othersthe set-up of the F/T sensor and accelerometer, e.g. of the F/TAS 30,and the related interface board(s). The software initialization taskincludes the steps of allocating resources such as memory for datastructures of the application, and other operating-system items (i.e.tasks, semaphores, message queues, clocks, etc.). If the hardware andsoftware initialization succeed, the system enters an IDLE state,waiting for the calibration command. Otherwise, the system enters aFAILED state as shown in FIG. 6 . The result of the initializationoperation can be communicated to the controller of the manipulator 10,either through a software event or through a function call returnedparameter.

State 2: IDLE state: the system waits for a command to start thecalibration process, which has been described in section “Calibrationprocedure”.

State 3: FAULT state: this state is entered in case of anysystem/software malfunction or in case of a detected safety risk, thesystem waits for a restart command. Upon entering the FAULT state, anasynchronous message or event is sent to the manipulator controller inorder to warn of this condition.

State 4: F/T_&_ACCELEROMETER_CALIBRATION state: In this state, themanipulator 10 is commanded through a predetermined set of poses withdifferent positions and orientations (see section “Calibrationprocedure”). In each pose, the F/T sensor and accelerometer data arerecorded upon the reception of a ‘record’ command. After the completionof the pose set, upon the reception of a ‘compute’ command, theaforementioned least-squares fitting technique, or any other suitableapproximation technique, is applied in order to calculate F/T sensor andaccelerometer offsets (F_(offsets) , T_(offsets) , LinAcc_(offsets) andAngAcc_(offsets) ) together with the coordinates of the centre ofgravity of the attached load. In the unlikely event the calculationfails, e.g. because of inconsistent results or because of a user-madeabort command of pose set moves, the system returns into the IDLE statewarning the manipulator controller of this event. Otherwise, at the endof the calibration phase, the system passes into theAPPLICATION_LOADS_EVALUATION state. In case of software or hardwarefailure detection, the system passes to the FAULT state.

State 5: APPLICATION_LOADS_EVALUATION state: In this state, a periodicprocess executes sequentially, but not necessarily in the given order,the following operations:

-   Data filtering, e.g. by means of a discrete Kalman filter cascade    for linear stochastic processes (see section “Sensor data    filtering”);-   Compensation of the effect of gravity and dynamic loads in F/T    sensor data (see section “Compensation of offsets and of gravity and    dynamics loads”);-   Determination, i.e. continuous updating based on manipulator 10    motion, of the position of the instrument 14 relative to the fulcrum    23 (see section “Determining the instrument position relative to the    fulcrum”)-   Calculate an estimate of the forces at the instrument tip 20 and at    the fulcrum respectively (see section “Calculating forces at the    instrument tip and at the fulcrum level”);    Optionally the following further operations are also executed by the    periodic process:-   Monitoring of compensated loads against predetermined maximum    threshold values e.g. stored in the real-time data-base. In case of    exceeding values, the function issues a warning message, or a stop    motion command and writes this condition in the real-time database;    this process can also be applied to the estimated forces at the    instrument tip 20 and at the fulcrum level (trocar 22) in order to    detect unsafe conditions or a failure of the F/TAS 30;-   Check the drift of sensor offsets (see section “Check of offset    drifts”);-   Monitoring the intra-abdominal insufflation pressure. In case of    depressurization, the function issues a warning message so that    appropriate action ca be taken, among which e.g. redefining the    position of the fulcrum 23.

FIG. 7 shows a possible sequence of the above operations in a flowchart. As seen in FIG. 7 , a primary linear Kalman filter, of cascadedconfiguration e.g. as described with respect to FIG. 4 , filters thesensor data prior to compensation of the “parasitic loads”. Aftercompensation, a secondary linear Kalman filter is applied to the forceand torque values, in order to further improve the smoothness quality ofthe signal at the input of the operation that calculates the forceestimate(s) (Compute F_(Tip) and F_(Fulcrum) ). Although shown in FIG. 7as executed before the step of calculating the force estimates, theoperation for determination of the instrument position can be executedperiodically at another point in the flow. Similarly one or more of theabove optional operations (indicated by block “ . . . ” in FIGS. 7 and 8) need not necessarily be executed subsequent to calculating the forceestimates.

FIG. 8 shows an alternative sequence of the above operations in a flowchart. As seen in FIG. 8 , a single filtering operation is appliedsubsequent to calculating the force estimate(s) (Compute F_(Tip) andF_(Fulcrum) ). The filtering operation can be based on a cascaded Kalmanfilter configuration as described with respect to FIG. 4 .

The alternative of FIG. 8 reduces the loss of information(under-/overrated loads) due to filtering, prior to calculation of theforce estimate(s), such that a further increase in accuracy can beachieved. The embodiment of FIG. 7 is preferable in case the system isconfigured for using the effector unit 12 as a control device(“joystick”) for assisted positioning of the manipulator 10 e.g. duringinsertion of the instrument 14.

In case, a request for recalibration is received, the state of thesystem is changed to F/T_&_ACCELEROMETER_CALIBRATION and the periodicprocess is stopped. In case of a software or hardware failure detection,the system is changed to the FAULT state and a warning is issued.

The execution rate of the cyclic process is configured according to theapplications requirements. For instance, when using the compensated datafor robotic tele-operation, this process shall preferably be run at thesame rate as that of the set-point generation for the manipulator 10,e.g. between 300 Hz and 1000 Hz.

Conclusion

The presented method/system provide a contribution to robotic and/orcomputer assisted minimally invasive surgery by offering an accurate andcost-effective way of estimating the contact forces at the instrumenttip and, optionally, at the trocar level.

In laboratory trials of a prototype system, an average estimation errorof 0.25N and a maximum estimation error of 0.65N have been determined.It will be appreciated, that even though these values were achievedusing a prototype under development, the estimation error level issatisfactory even for most tasks in surgical laparoscopy, since 0.25N isbelow the sensitivity threshold of the human hand. Furthermore, it willbe appreciated the a total signal delay of 50 ms achieved with theprototype make the system readily suitable for tele-operation.

1. A method of operating a minimally invasive medical system having a manipulator and a surgical instrument to determine coordinates of an external fulcrum that limits said surgical instrument in motion, said method comprising positioning a tip of a surgical instrument through an incision into a body cavity, said surgical instrument having an instrument shaft with a longitudinal instrument axis Z; mounting the surgical instrument to a manipulator, the manipulator including a force/torque sensor; moving said minimally invasive instrument along a first axis X and a second axis Y, each of said first and second axes perpendicular to the instrument axis Z, until a reaction force along each axis is below a given threshold, and defining X and Y axis coordinates of the external fulcrum at a location where the reaction forces along said both axes are below the given threshold; and determining a Z axis coordinate position of the external fulcrum along said instrument axis by pivoting the instrument with respect to its tip until a sufficient contact force is reached, and determining the Z axis coordinate position using the lever principle.
 2. The method of claim 1, wherein: determining the Z axis coordinate position of the external fulcrum comprises: pivoting the instrument in a first direction until a sufficient first contact force is reached; measuring the module of a first moment vector and the module of a first force vector corresponding to said first contact force; calculating a first position of the external fulcrum along said instrument axis by dividing said first moment vector module by said first force vector module; pivoting the instrument in a second direction, which is opposite direction to said second direction, until a sufficient second contact force is reached; measuring the module of a second moment vector and the module of a second force vector corresponding to said second contact force; calculating a second position of the external fulcrum along said instrument axis by dividing said second moment vector module by said second force vector module; and setting an initial reference distance vector (D_(Fulcrum 0)) using the mean value of said calculated first position and said calculated second position.
 3. The method of claim 1, further including, after determining the coordinates of the external fulcrum, controlling movement of said instrument by the manipulator with respect to said external fulcrum.
 4. The method of claim 1, wherein said manipulator includes a 6-DOF accelerometer, and said method further comprises the steps: measuring by means of said 6-DOF accelerometer a gravity load and/or dynamic loads exerted onto said 6-DOF force/torque sensor; and compensating said gravity and/or dynamic loads in said measured force and said measured torque.
 5. The method of claim 3, further including, after determining the coordinates of the external fulcrum, continuously determining an updated position of said instrument relative to the fulcrum using manipulator motion information.
 6. The method of claim 1, wherein said given threshold has a value of about 0.3N and/or said sufficient contact force has a value of about 3N.
 7. The method of claim 1, further comprising applying a linear Kalman filter to force and torque data measured by said force/torque sensor.
 8. The method of claim 1, further comprising computing the location of the external fulcrum with respect to a fixed reference frame through a transformation of coordinates from a reference frame of the sensor to a fixed reference frame.
 9. The method of claim 1, wherein the force/torque sensor is a 6 DOF force/torque sensor.
 10. The method of claim 1, wherein positioning the tip of the surgical instrument includes passing the tip through a trocar positioned in the incision. 